;/*=============================================================================
;#    The element-comm is a tentative program used to test our ideas.           
;#                       Copyright (C)  bgao                                    
;#
;#     This program is free software; you can redistribute it and/or            
;#      modify it under the terms of the GNU General Public License             
;#     as published by the Free Software Foundation; either version 2           
;#         of the License, or (at your option) any later version.               
;#
;#     This program is distributed in the hope that it will be useful,          
;#    but WITHOUT ANY WARRANTY; without even the implied warranty of           
;#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the            
;#             GNU General Public License for more details.                    
;#
;#    You should have received a copy of the GNU General Public License        
;#     along with this program; if not, write to the Free Software             
;#      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,             
;#                       MA  02110-1301, USA.                                  
;#==============================================================================
;# $Id$ 
;#
;# $Author$ 
;#
;# $Revision$ 
;#
;# $Date$ 
;#
;# Description: 
;#
;=============================================================================*/
	GET option.inc
	GET 2410addr.inc

	IMPORT	wsn_start
	IMPORT	IsrIRQ
	IMPORT  |Image$$KERNEL_ZI$$ZI$$Base|   ; Base and limit of area
	IMPORT  |Image$$KERNEL_ZI$$ZI$$Limit|  ; to zero initialise	

 if :def: __ARMCC_REALVIEW2_2
	PRESERVE8
	REQUIRE8	{TRUE}
 endif

    MACRO
$HandlerLabel HANDLER $HandleLabel

$HandlerLabel
	sub		sp,sp,#4        ;decrement sp(to store jump address)
	stmfd	sp!,{r0}        ;PUSH the work register to stack(lr does't push because it return to original address)
	ldr     r0,=$HandleLabel;load the address of HandleXXX to r0
	ldr     r0,[r0]         ;load the contents(service routine start address) of HandleXXX
	str     r0,[sp,#4]      ;store the contents(ISR) of HandleXXX to stack
	ldmfd   sp!,{r0,pc}     ;POP the work register and pc(jump to ISR)
	MEND


 
	area BOOT, code, readonly
	entry
boot_wsn_start
    ldr pc,	Reset
    ldr pc,	HandlerUndef
    ldr pc,	HandlerSWI
    ldr pc,	HandlerPrefetchAbort
    ldr pc,	HandlerDataAbort
    ldr pc,	HandlerNotUsed
    ldr pc,	HandlerIRQ
    ldr pc,	HandlerFIQ

Reset					DCD WSNReset
HandlerUndef			DCD WSNHandlerUndef
HandlerSWI				DCD	WSNHandlerSWI
HandlerPrefetchAbort	DCD	WSNHandlerPrefetchAbort
HandlerDataAbort		DCD	WSNHandlerDataAbort
HandlerNotUsed			DCD	WSNHandlerNotUsed
HandlerIRQ				DCD	WSNHandlerIRQ
HandlerFIQ				DCD	WSNHandlerFIQ

	LTORG   
WSNHandlerFIQ      HANDLER HandleFIQ
WSNHandlerIRQ      HANDLER HandleIRQ
WSNHandlerUndef    HANDLER HandleUndef
WSNHandlerSWI      HANDLER HandleSWI
WSNHandlerDataAbort  HANDLER HandleDataAbort
WSNHandlerPrefetchAbort   HANDLER HandlePrefetchAbort
WSNHandlerNotUsed	HANDLER HandleNotUsed


 

Copy_vector
	ldr r0,=0x30008000
	ldr r1,=0x00000000
	ldmia r0!,{r4-r11}  ;copy the vector (8 words)
	stmia r1!,{r4-r11}
	ldmia r0!,{r4-r11}  ;copy the pointer table of handler (8 words)
	stmia r1!,{r4-r11}
	mov	pc,lr 
 
;======================================================
; ENTRY  
;======================================================

WSNReset
    ; Perform a soft reset of the system.  Put the CPU into the
    ; same state as it would be if it had been reset
;    mov	r0, #0
;    mcr	p15, 0, r0, c7, c7, 0		;@ invalidate I,D caches
;    mcr	p15, 0, r0, c7, c10, 4		;@ drain WB
;    mcr	p15, 0, r0, c8, c7, 0		;@ invalidate I & D TLBs
;    mrc	p15, 0, r0, c1, c0, 0		;@ ctrl register
;    bic	r0, r0, #0x000f			;@ ............wcam
;    bic	r0, r0, #0x1100			;@ ...i...s........
;    mcr	p15, 0, r0, c1, c0, 0		;@ ctrl register
;    add	sp, pc, #0x7F0			; Set stack
    
   
	;watch dog disable
    ldr	r0,=WTCON        
	ldr	r1,=0x0         
	str	r1,[r0]

	;all interrupt disable
	ldr	r0,=INTMSK
	ldr	r1,=0xffffffff  
	str	r1,[r0]

	;all sub interrupt disable
	ldr	r0,=INTSUBMSK
	ldr	r1,=0x7ff		
	str	r1,[r0]

    ;To reduce PLL lock time, adjust the LOCKTIME register. 
    ldr r0, =LOCKTIME
    ldr r1, =0xffffff
    str r1, [r0]
	
    ;Config slow clock mode
    ldr r0, =CLKSLOW
    ldr r1, =4;
    str r1, [r0];
              
    ;Configure MPLL = 200M
    ldr r0, =MPLLCON          
    ldr r1, =((M_MDIV<<12)+(M_PDIV<<4)+M_SDIV)
    str r1, [r0]
    
    ;Config div for PCLK
    ldr r0, =CLKDIVN
    ldr r1, =((M_HDIVN<<1)+(M_PDIVN))
    str r1, [r0]

	;Check if the boot is caused by the wake-up from POWER_OFF mode.
	;ldr	r1,=GSTATUS2
	;ldr	r0,[r1]
	;tst	r0,#0x2
        ;In case of the wake-up from POWER_OFF mode, go to POWER_OFF_WAKEUP handler. 
	;bne	WAKEUP_POWER_OFF

	;Initialize stacks
	bl InitStacks
	
	; Setup IRQ handler
	ldr	r0,=HandleIRQ       ;This routine is needed
	ldr	r1,=IsrIRQ          ;if there isn't 'subs pc,lr,#4' at 0x18, 0x1c'
	str	r1,[r0]

	ldr	r0,=HandleUndef       ;This routine is needed
	ldr	r1,=IsrUndef          ;if there isn't 'subs pc,lr,#4' at 0x18, 0x1c'
	str	r1,[r0]

	ldr	r0,=HandleDataAbort       ;This routine is needed
	ldr	r1,=IsrDataAbort          ;if there isn't 'subs pc,lr,#4' at 0x18, 0x1c'
	str	r1,[r0]

	ldr	r0,=HandlePrefetchAbort       ;This routine is needed
	ldr	r1,=IsrPrefetchAbort          ;if there isn't 'subs pc,lr,#4' at 0x18, 0x1c'
	str	r1,[r0]

	;Copy Vector to 0x00000000
	bl Copy_vector
	
	;Clear BSS
    bl InitRam   

	;jump to c init function
	ldr pc,=wsn_start


;function initializing stacks
InitStacks
	;Don't use DRAM,such as stmfd,ldmfd......
	;SVCstack is initialized before
	;Under toolkit ver 2.5, 'msr cpsr,r1' can be used instead of 'msr cpsr_cxsf,r1'
	mrs	r0,cpsr
	bic	r0,r0,#MODEMASK
	orr	r1,r0,#UNDEFMODE|NOINT
	msr	cpsr_cxsf,r1		;UndefMode
	ldr	sp,=UndefStack
	
	orr	r1,r0,#ABORTMODE|NOINT
	msr	cpsr_cxsf,r1		;AbortMode
	ldr	sp,=AbortStack

	orr	r1,r0,#IRQMODE|NOINT
	msr	cpsr_cxsf,r1		;IRQMode
	ldr	sp,=IRQStack
    
	orr	r1,r0,#FIQMODE|NOINT
	msr	cpsr_cxsf,r1		;FIQMode
	ldr	sp,=FIQStack

	bic	r0,r0,#MODEMASK|NOINT
	orr	r1,r0,#SVCMODE
	msr	cpsr_cxsf,r1		;SVCMode
	ldr	sp,=SVCStack
	
	;USER mode has not be initialized.
	
	mov	pc,lr 
	;The LR register won't be valid if the current mode is not SVC mode.


InitRam	
    ldr r3, BaseOfZero
    ldr r1, EndOfBSS; Top of zero init segment
    mov r2, #0
1
    cmp     r3, r1      ; Zero init
    strcc   r2, [r3], #4
    bcc     %B1

	mov pc, lr	
	

	
IsrUndef
1	b	%b1		; infinite loop
IsrDataAbort
1	b	%b1		; infinite loop
IsrPrefetchAbort
1	b	%b1		; infinite loop
	LTORG

BaseOfZero	DCD	|Image$$KERNEL_ZI$$ZI$$Base|
EndOfBSS	DCD	|Image$$KERNEL_ZI$$ZI$$Limit|	
	
    ALIGN
    AREA RamData, DATA, READWRITE

        ^   _ISR_STARTADDRESS
HandleReset 	#   4
HandleUndef 	#   4
HandleSWI   	#   4
HandlePrefetchAbort    #   4
HandleDataAbort    #   4
HandleNotUsed  #   4
HandleIRQ   	#   4
HandleFIQ   	#   4


	end
